#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/Float32MultiArray.h"
#include "std_msgs/Float64.h"
#include "std_msgs/Int32MultiArray.h"
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>

#include "FalconSDK.h"
#include "FalconPID.h"

#include <unistd.h>
#include <time.h>
#include <iostream>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include <sstream>
#include <vector>
#include<sys/stat.h>
#include<sys/types.h>

#define VEL2FALCON_SCALE 0.6//0.4
#define MAX_DISP 0.4
#define PI 3.1415926

using namespace std;
using namespace libnifalcon;//falcon frame 0 1 2 (y,-z,-x) thumb back, m-finger up

double pos[3]={0,0,0},ppos[4]={0,0,0,0};
double pos_buffer[3]={0,0,0};
double rotation=0;
double envforce[3]={0,0,0};

double out_force[4]={0,0,0,0};
double vel_ref[4] = {0};

geometry_msgs::TwistStamped force_msg;

void VelCallback(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
	pos_buffer[0] = -msg->twist.linear.y*VEL2FALCON_SCALE;		//1Y in falcon frame
	pos_buffer[1] = 0;//msg->twist.linear.z*VEL2FALCON_SCALE;		//0x in falcon frame
	pos_buffer[2] = -msg->twist.linear.x*VEL2FALCON_SCALE;		//2z in falcon frame 

	//ROS_INFO("x:%1.3f,y:%1.3f,z:%1.3f",pos_buffer[0],pos_buffer[1],pos_buffer[2]);
}

int main(int argc, char **argv)
{
	FalconPID fpid;		// class declarition
	fpid.option=2;		// enable the velocity mapping under admittance mode

	fpid.fal_open();	// open device 
	fpid.init();		// initiating device

	ros::init(argc, argv, "falcon");

	ros::NodeHandle n;

	ros::Publisher falcon_pub = n.advertise<geometry_msgs::TwistStamped>("vel_ref", 100);
	ros::Subscriber vel_sub = n.subscribe("/mavros/local_position/velocity_local", 100, VelCallback);

	ros::Rate loop_rate(1000);
	double time = ros::Time::now().toSec();

	int count = 0;
	while (ros::ok()){
		static double buf_eforce[3]={0,0,0};
		if (pos_buffer[0]>MAX_DISP){	
			pos[0] = MAX_DISP;
		}else{
			if (pos_buffer[0]<-MAX_DISP){
				pos[0] = -MAX_DISP;
			}else{
				pos[0] = pos_buffer[0];
			}
		}
		if( pos_buffer[1]>MAX_DISP){	
			pos[1] = MAX_DISP;
		}else		{
			if( pos_buffer[1]<-MAX_DISP){
				pos[1] = -MAX_DISP;
			}else{
				pos[1] = pos_buffer[1];
			}
		}
		if( pos_buffer[2]>MAX_DISP){	
			pos[2] = MAX_DISP;
		}else{
			if( pos_buffer[2]<-MAX_DISP){
				pos[2] = -MAX_DISP;
			}else{
				pos[2] = pos_buffer[2];
			}
		}


	//ROS_INFO("pos x:%1.3f,y:%1.3f,z:%1.3f",pos[0],pos[1],pos[2]);
		
		fpid.run(pos, out_force, buf_eforce, ppos); // call falcon pid controller, servo the velocity position, and measure the force input from user

	//ROS_INFO("ppos x:%1.3f,y:%1.3f,z:%1.3f",-ppos[2]+0.12,ppos[0],-ppos[1]); //0,1,2 -> (y,-z,-x) thumb back, m-finger up

		if (ppos[3]<0){
			ppos[3] += 2*PI;
		}else{
			if (ppos[3]>2*PI){
				ppos[3] -= 2*PI; 
			}
		}

/*
		force_msg.data.push_back((0.125-ppos[2])*40);// x in body fixed frame
		force_msg.data.push_back(ppos[0]*40);// y in body fixed frame
		force_msg.data.push_back(ppos[1]*24);// z in body fixed frame
		force_msg.data.push_back(ppos[3]);// yaw in body fixed frame
		force_msg.data.push_back(time);
*/

		time = ros::Time::now().toSec();
	
		vel_ref[0] = -out_force[2]/4.0*3.0;
		if(vel_ref[0] > 5){
			vel_ref[0]  = 5;
		}
		if(vel_ref[0] < -5){
			vel_ref[0]  = -5;
		}

		vel_ref[1] =  out_force[0]/5.0*3.0;
		if(vel_ref[1] > 5){
			vel_ref[1]  = 5;
		}
		if(vel_ref[1] < -5){
			vel_ref[1]  = -5;
		}

		vel_ref[2] =  out_force[1]/4.0*3.0;
		if(vel_ref[2] > 5){
			vel_ref[2]  = 5;
		}
		if(vel_ref[2] < -5){
			vel_ref[2]  = -5;		
		}
		
		if(ppos[3] > PI)
			vel_ref[3] = ppos[3] - PI * 2;
		else if(vel_ref[3] < -PI)
			vel_ref[3] = ppos[3] + PI * 2;
		else
			vel_ref[3] = ppos[3];

		//cout<<"px: "<<(0.12-ppos[2])*24<<",py: "<<ppos[0]*24<<",pz: "<<ppos[1]*24<<", yaw: "<<vel_ref[3]<<endl;

		force_msg.twist.linear.x = vel_ref[0]*0.2;//x in body fixed frame
		force_msg.twist.linear.y = -vel_ref[1]*0.2;// y in body fixed frame
		force_msg.twist.linear.z = vel_ref[2]*0.2;// 0
		force_msg.twist.angular.x = 0;// yaw in body fixed frame
		force_msg.twist.angular.y = 0;// z in body fixed frame
		force_msg.twist.angular.z = 0;// z in body fixed frame
	

		falcon_pub.publish(force_msg);

		ros::spinOnce();
		loop_rate.sleep();
		++count;
	}

	fpid.fal_close();
	return 0;
}
